/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "dma.h"
#include "gpio.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "Dirver_Can.h"
#include "Driver_Usart.h"
#include "PID.h"
#include "oled.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

const motor_measure_t *motor_data; // 声明电机结构体指针

extern PID_TypeDef SpeedPID_Struction;    // 速度环PID结构体
extern PID_TypeDef CurrentPID_Struction;  // 电流环PID结构体
extern PID_TypeDef PositionPID_Struction; // 位置环PID结构体

/******************************PID参数结构体**************************************/
typedef struct {
  float P;
  float I;
  float D;
} PIDValue;

PIDValue Speed_Loop_PID;
PIDValue Current_Loop_PID;
PIDValue Position_Loop_PID;
/****************************************************************************/

uint8_t Time_Cnt = 0;

/******************************期望值初始化**************************************/
uint16_t Target_Speed = 5000;
uint16_t Target_Position = 1000;
/****************************************************************************/

// 串口接收数组
char RxData[20];
char Target[10] = "Target";

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);

/* USER CODE BEGIN PFP */

void Motor_Forward(float v);
// void DiPan_Motor_backward(float v);
void DiPan_Motor_Stop();

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void) {
  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick.
   */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_CAN1_Init();
  MX_I2C1_Init();
  MX_USART2_UART_Init();
  MX_TIM1_Init();
  /* USER CODE BEGIN 2 */
  HAL_TIM_Base_Start_IT(&htim1);
  OLED_Init();
  OLED_Clear();
  // 滤波器初始化
  can_filter_init();
  CAN_Init(&hcan1);

  /******************************PID初始化**************************************/
  pid_init(&SpeedPID_Struction);
  pid_init(&CurrentPID_Struction);
  pid_init(&PositionPID_Struction);
  /******************************PID调参处**************************************/
  // 速度环
  Speed_Loop_PID.P = 8;
  Speed_Loop_PID.I = 0.1;
  Speed_Loop_PID.D = 2;
  // 电流环
  Current_Loop_PID.P = 1;
  Current_Loop_PID.I = 0.002;
  Current_Loop_PID.D = 0.2;
  // 位置环
  Position_Loop_PID.P = 30;
  Position_Loop_PID.I = 0.001;
  Position_Loop_PID.D = 0;
  /**************************PID参数初始化,分别是:********************************/
  /*****参数结构体、ID、最大输出、积分限幅值、死区、控制周期、最大误差、目标值、三个PID参数**/
  /******************************PID初始化设置处*********************************/
  // 速度环
  SpeedPID_Struction.f_param_init(&SpeedPID_Struction, PID_Speed, 16000, 16000,
                                  10, 0, 8000, 0, Speed_Loop_PID.P,
                                  Speed_Loop_PID.I, Speed_Loop_PID.D);
  // 电流环
  CurrentPID_Struction.f_param_init(&CurrentPID_Struction, PID_Current, 16000,
                                    8000, 10, 0, 8000, 0, Current_Loop_PID.P,
                                    Current_Loop_PID.I, Current_Loop_PID.D);
  // 位置环
  PositionPID_Struction.f_param_init(
      &PositionPID_Struction, PID_Position, 16000, 2000, 1, 0, 8000, 0,
      Position_Loop_PID.P, Position_Loop_PID.I, Position_Loop_PID.D);
  /****************************************************************************/

  // 串口接收初始化
  HAL_UARTEx_ReceiveToIdle_DMA(&huart2, RxData, sizeof(RxData));
  __HAL_DMA_DISABLE_IT(&hdma_usart2_rx, DMA_IT_HT);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  motor_data = get_chassis_motor_measure_point(0);
  Usart2_Printf("Start\r\n");
  while (1) {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    Motor_Forward(Target_Position);
    //    Motor_Forward(Target_Speed);
    HAL_Delay(2);
  }
  /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void) {
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
   */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
   * in the RCC_OscInitTypeDef structure.
   */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 8;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
   */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK |
                                RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

// 电机控制接口函数

void Motor_Forward(float Target) {

  /***********************************位置环************************************/
  PositionPID_Struction.target = Target; // 位置环的期望是目标位置
  // 输入当前位置值
  PositionPID_Struction.f_cal_pid(&PositionPID_Struction, motor_data->angle);
  /****************************************************************************/

  /***********************************速度环************************************/
  SpeedPID_Struction.target =
      PositionPID_Struction.output; // 速度环输入是位置环输出
  // 输入当前速度值
  SpeedPID_Struction.f_cal_pid(&SpeedPID_Struction,
                               motor_data->speed_rpm); // 速度环输入
  /****************************************************************************/

  /***********************************电流环************************************/
  CurrentPID_Struction.target =
      SpeedPID_Struction.output; // 电流环输入是速度环输出
  // 输入电流实测值
  CurrentPID_Struction.f_cal_pid(&CurrentPID_Struction,
                                 motor_data->given_current);
  /****************************************************************************/

  // 电流环输出作为CAN发送值
  CAN_cmd_chassis(CurrentPID_Struction.output, 0, 0, 0);

  HAL_Delay(2);
}

// void DiPan_Motor_backward(float v) {
//   // 设置各电机的的目标转速值（单位rpm）
//   SpeedPID_Struction.target = -v; //
//
//   SpeedPID_Struction.f_cal_pid(&SpeedPID_Struction,
//                                motor_data->speed_rpm); //
//                                1号电机的pid电流计算值
//
//   CAN_cmd_chassis(SpeedPID_Struction.output, 0, 0, 0);
//   HAL_Delay(2);
// }
//
void DiPan_Motor_Stop() {
  CAN_cmd_chassis(0, 0, 0, 0);
  HAL_Delay(2);
}

/**
 * 每50ms进入中断函数
 * @param htim
 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
  if (htim == &htim1) {
    /****************************判断电机是否在线********************************/
    if (ConnectFlag == Pre_ConnectFlag) {
      SpeedPID_Struction.err = 0;
    }
    Pre_ConnectFlag = ConnectFlag;
    /**************************************************************************/
    Usart2_Printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", motor_data->ecd,
                  motor_data->last_ecd, motor_data->given_current,
                  motor_data->speed_rpm, motor_data->angle, Target_Position,
                  Target_Speed, (uint16_t)SpeedPID_Struction.iout,
                  (uint16_t)CurrentPID_Struction.iout,
                  (uint16_t)SpeedPID_Struction.output,
                  (uint16_t)CurrentPID_Struction.output,
                  (uint16_t)PositionPID_Struction.iout,
                  (uint16_t)PositionPID_Struction.output);
  }
}

/**
 * 串口接收中断服务函数
 * @param huart
 * @param Size
 */
/***********************************PID参数************************************/
//// 速度环
// float Speed_P = 8;
// float Speed_I = 0.1;
// float Speed_D = 0;
//// 电流环
// float Current_P = 1;
// float Current_I = 0.8;
// float Current_D = 0;
//// 位置环
// float Position_P = 10;
// float Position_I = 0.1;
// float Position_D = 0.1;
//
//// 速度
// char S_P[4] = "S_P";
// char S_I[4] = "S_I";
// char S_D[4] = "S_D";
//// 电流
// char C_P[4] = "C_P";
// char C_I[4] = "C_I";
// char C_D[4] = "C_D";
//// 位置
// char P_P[4] = "P_P";
// char P_I[4] = "P_I";
// char P_D[4] = "P_D";
/**************************************************************************/
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) {
  if (huart == &huart2) {

    HAL_UART_Transmit_DMA(&huart2, RxData, Size);

    HAL_UARTEx_ReceiveToIdle_DMA(&huart2, RxData, sizeof(RxData));

    __HAL_DMA_DISABLE_IT(&hdma_usart2_rx, DMA_IT_HT);

    //    Target_Speed = Change_ReceiveData(RxData, Target, 6, Target_Speed, 4);
    Target_Position = Change_ReceiveData(RxData, Target, 6, Target_Position, 4);
    if (Target_Position > 10000) {
      Target_Position = Target_Position / 10;
    }
  }
}
/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void) {
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1) {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line) {
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line
     number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file,
     line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
